package org.electrode.base;

import edu.wpi.first.wpilibj.Preferences;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

public class MotorChange {
	private final double left;
	private final double right;

	public MotorChange(double left, double right) {
		this.left = left;
		this.right = right;
	}

	public double getLeft() {
		return -1 * axisCorrection(left);
	}

	public double getRight() {
		return axisCorrection(right);
	}

	private double axisCorrection(double x) {
		double cubed = x * x * x;
		if (x < 0) {
			return Math.max(-1.0, -getMotorDeadZoneConstant() + cubed);
		} else if (x > 0) {
			return Math.min(1.0, getMotorDeadZoneConstant() + cubed);
		} else {
			return 0;
		}
	}

	public void updateDashboard() {
		SmartDashboard.putNumber("LeftMotor", getLeft());
		SmartDashboard.putNumber("RightMotor", getRight());
	}

	private double getMotorDeadZoneConstant() {
		return Preferences.getInstance().getDouble("MotorDeadZoneConstant",
				0.125);
	}
}
